/*
 * FineLocalization.c
 *
 *  Created on: Nov 27, 2012
 *      Author: pmelick
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define WIDTH 40.0//Width of the visible area of the antenna in cm
#define RADIUS 42.0//Distance of the tag from the center of the secondary coil
#define TAGANGLE 45.0*3.1416/180.0//Angle of the tags from the center of the secondary coil as measured from the forward direction of the car
#define PI 3.1416

float cot(float);

static float xCar;
static float yCar;
static float phiCar;

/* The localization function for the RDIF antenna.
 * x0 and y0 are the coarse localization guesses for the car location.
 * Thetaij is the jth angle from the ith tag.  For example, theta 12 is the
 * angle at which the first tag was no longer visible.
 * Once this function has run, the fine localized position of the car
 * can be returned via the three access functions below.
 */

void fineLocalize( float x0, float y0, float theta11, float theta12, float theta21, float theta22)
{
	float x1;
	float y1;
	float x2;
	float y2;
	float a;
	float b;

	//Convert to radians
	theta11=theta11*PI/180.;
	theta12=theta12*PI/180.;
	theta21=theta21*PI/180.;
	theta22=theta22*PI/180.;

	x1=x0+(WIDTH/2)*((1/sin(theta11)+(1/sin(theta12)))*(1/(cot(theta11)-cot(theta12))));
	y1=y0+cot(theta11)*(x1-x0)-(WIDTH/2)*(1/sin(theta11));
	x2=x0+(WIDTH/2)*((1/sin(theta21)+(1/sin(theta22)))*(1/(cot(theta21)-cot(theta22))));
	y2=y0+cot(theta22)*(x2-x0)+(WIDTH/2)*(1/sin(theta22));
	phiCar=atan((y1-y2)/(x2-x1));
	a=RADIUS*sin(phiCar+TAGANGLE);
	b=RADIUS*cos(phiCar+TAGANGLE);
	xCar=(x1+x2+b-a)/2;
	yCar=(y1+y2-a-b)/2;
}

float getXCar(void)
{
	return xCar;
}

float getYCar(void)
{
	return yCar;
}

float getPhiCar(void)
{
	return phiCar;
}

float cot(float theta)
{
	return 1./(tan(theta));
}
